ent Vision-based Loop Closing Techniques for Delayed State Robot Mapping

This paper loop closing for Simulta experiments show that mates maintained with a Filter are consistent eno based pose constraints fo information links are ad computes relative pose minimisation of 3D poin obtained from the matc image pairs. We propos for closeness of means a the same time. I. Closing large loops d Mapping (SLAM) is q plished, it reduces the mously. A straight for problem is to rely on t choice (be it a Kalma particle filter) and perf and as often as possib based on the likelihood of the problems associ such as aliasing for hom closing all loops consis might add information unreliable. Unnecessary links a information to reduce t with repetitive measure little or no motion occ small steps are used i resentation. Unreliable that are not consisten which the system was when distance estimates Distance errors being larger for measurement introduce errors that are Thus it is important hand, and reliably on t that testing for loop clo independently of the ve a nice thing to do once this work, we adopt ins relying on the estimator The authors are with th dustrial, CSIC-UPC. Lloren [vila,cetto,sanfel ent Vision-based Loop Closing Techniques for Delayed State Robot Mapping Viorela Ila, Juan Andrade-Cetto and Alberto Sanfeliu shows results on outdoor vision-based neous Localization and Maping. Our for loops of over 50m, the pose estiDelayed-State Extended Information ugh to guarantee assertion of visionr loop closure, provided no necessary ded to the estimator. The technique constraints via a robust least squares t correspondences, which are in turn hing of SIFT features over candidate e a loop closure test that checks both nd for highly informative updates at INTRODUCTION uring Simultaneous Localisation and uite challenging. But, when accomaccumulated estimation error enorward solution to the loop closing he pose estimates from the filter of n filter, an Information filter, or a orm data association tests as much le. By testing for data association of estimates, one can avoid some ated with appearance-based SLAM, ogeneous or repetitive scenes. But, tent with the likelihood of estimates links that are either unnecessary or re those that contribute with little he estimation error. That is the case ments to the same landmarks when urs, or when pose constraints from n the case of a delayed state replinks on the other hand, are those t with the sensor uncertainties for trained. This happens for example come from computer vision sensors. inverse to disparity in images are s to far away landmarks, and might inconsistent. to close loops sparsly on the one he other. In [1] the authors suggest sure in SLAM should be performed hicle pose estimates. This is certainly the filter has become inconsistent. In tead the straightforward approach of for the generation of pose constraint e Institut de Robòtica i Informàtica Ins Artigas 4-6, Barcelona, 08028 Spain. iu]@iri.upc.edu. hypotheses, but paying special attention no to let the system become overconfident too soon. Since adding information links for all possible matches produces overconfident estimates that in the long term lead to filter inconsistency, we propose instead a two step loop closure test. First, we check whether two poses are candidates for loop closure with respect to their mean estimates. This is achieved by testing for the Mahalanobis distance in the same way data association gating is commonly performed during a SLAM update. But instead of adding all these information links, we limit the candidates to a second test that checks for large values on the second term of the Bhattacharyya distance. The purpose of this part of the test is to allow loop closing only on highly informative situations. That is, when the proposed matching covariances are sufficiently different, and a large amount of information is expected to enter the filter. The reminder of this paper is as follows. In section II we present our strategy for computing pose constraints from two vantage points using computer vision. Section III is devoted to explain our chosen SLAM representation, and the way in which our vision-based pose constraints are used to update the map. Section IV described the proposed loop closure test. Some experimental results in an outdoor scenario are shown in Section V, and concluding remarks are added in Section VI. II. RELATIVE POSE CONSTRAINTS The technique iterates as follows: SIFT image features are extracted and matched from candidate stereo image pairs. Their image point correspondences are then triangulated to obtain a set of 3D feature matches, which are in turn used to compute a least squares best fit pose transformation. Robust feature outlier rejection is obtained via RANSAC during the computation of the best camera pose constraint. These camera pose constraints are used as relative pose measurements in a delayed-state information-form SLAM. A substantial computational complexity advantage of the delayed-state information-form SLAM is that predictions and updates take constant time prior to loop closure given its exact sparseness [2]. Thanks to the features used, the proposed technique is robust enough not only to relate consecutive image pairs during robot motion, but also, to assert loop closure hypotheses. A. Feature Extraction Simple correlation-based features, such as Harris corners [3] or Shi and Tomasi features [4], are of common use in vision-based SFM and SLAM; from the early uses of Harris himself to the popular work of Davison [5]. This kind of features can be robustly tracked when camera displacement is small and are tailored to real-time applications. However, given their sensitivity to scale, their matching is prone to fail under larger camera motions; less to say for loopclosing hypotheses testing. Given their scale and local affine invariance properties, we opt to use SIFTs instead [6], [7], as they constitute a better option for matching visual features from varying poses. To deal with scale and affine distortions in SIFTs, keypoint patches are selected from difference-ofGaussian images at various scales, for which the dominant gradient orientation and scale are stored. In our system, image pairs are acquired from a calibrated stereo rig1. Features are extracted and matched with previous image pairs. The surviving features are then stereo triangulated enforcing epipolar and disparity constraints. The epipolar constraint is enforced by allowing feature matches only within ±1 pixel rows on rectified images. The disparity constraint is set to allow matches within a 1−10 meter range, where camera resolution is best. The result is a set of two clouds of matching 3D points pt from the current pose, and pi from a previous pose, 0 < i < t, both referenced to the coordinate frame of the left camera. B. Pose Estimation The homogeneous transformation relating the two aforementioned clouds of points can be computed by solving a set of equations of the form pt = Rpi + t . (1) A solution for the rotation matrix R is computed by minimising the sum of the squared errors between the rotated directional vectors2 of feature matches for the two robot poses. The solution to this minimisation problem gives an estimate of the orientation of one cloud of points with respect to the other, and can be expressed in quaternion form as ∂ ∂R (